#ifndef MQTT2ROS_HPP
#define MQTT2ROS_HPP

#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "motion_interface_msg.hpp"

#include "mqtt_interface.hpp"

namespace mqtt2ros {
class Mqtt2Ros : public rclcpp::Node {

public:
  Mqtt2Ros()
    : Node("mqtt2ros_node")
  {

    pub_pose_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/robot_pose", 10);

    // 定义消息回调函数
    MqttInterface::Callback_t callback = [this](const mqtt::const_message_ptr& msg) {

      motion_interface::rig_pos_pack_t rig_pos;
      memcpy(&rig_pos, msg->get_payload_str().data(), sizeof(motion_interface::rig_pos_pack_t));

      geometry_msgs::msg::PoseStamped pose_msg;
      pose_msg.header.frame_id = "map";
      pose_msg.header.stamp.sec = rig_pos.sec;
      pose_msg.header.stamp.nanosec = rig_pos.nsec;
      pose_msg.pose.position.x = rig_pos.rig_pos.pos_X / 1000; // from m to mm
      pose_msg.pose.position.y = rig_pos.rig_pos.pos_Y / 1000;
      pose_msg.pose.position.z = 0.0;
      pose_msg.pose.orientation.x = 0.0;
      pose_msg.pose.orientation.y = 0.0;
      pose_msg.pose.orientation.z = 0.0;
      pose_msg.pose.orientation.w = 1.0; // 默认四元数表示无旋转
      pub_pose_->publish(pose_msg);
    };

    // 启动MQTT相关
    MqttInterface::MqttConfigure_t mqtt_config = {
      .broker = "tcp://192.168.8.45:1883",
      .client_id = "FineRobot_2",
      .topic = "/rig6/down/pos",
      .callback = callback,
    };
    mqtt_interface_ptr_ = std::make_unique<MqttInterface>(mqtt_config);

    if (!mqtt_interface_ptr_->connect()) {
      RCLCPP_ERROR(this->get_logger(), "MQTT连接失败");
      rclcpp::shutdown();
      return;
    } else {
      RCLCPP_INFO(this->get_logger(), "MQTT连接成功");
    }
  }

private:

  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
  std::unique_ptr<MqttInterface> mqtt_interface_ptr_;




};

} // namespace mqtt2ros


#endif //MQTT2ROS_HPP
